home *** CD-ROM | disk | FTP | other *** search
/ Skunkware 5 / Skunkware 5.iso / lib / calc / quat.cal < prev    next >
Text File  |  1995-07-17  |  3KB  |  217 lines
  1. /*
  2.  * Copyright (c) 1993 David I. Bell
  3.  * Permission is granted to use, distribute, or modify this source,
  4.  * provided that this copyright notice remains intact.
  5.  *
  6.  * Routines to handle quaternions of the form:
  7.  *    a + bi + cj + dk
  8.  *
  9.  * Note: In this module, quaternians are manipulated in the form:
  10.  *    s + v
  11.  * Where s is a scalar and v is a vector of size 3.
  12.  */
  13.  
  14. obj quat {s, v};        /* definition of the quaternion object */
  15.  
  16.  
  17. define quat(a,b,c,d)
  18. {
  19.     local obj quat    x;
  20.  
  21.     x.s = isnull(a) ? 0 : a;
  22.     mat x.v[3];
  23.     x.v[0] = isnull(b) ? 0 : b;
  24.     x.v[1] = isnull(c) ? 0 : c;
  25.     x.v[2] = isnull(d) ? 0 : d;
  26.     return x;
  27. }
  28.  
  29.  
  30. define quat_print(a)
  31. {
  32.     print "quat(" : a.s : ", " : a.v[0] : ", " : a.v[1] : ", " : a.v[2] : ")" :;
  33. }
  34.  
  35.  
  36. define quat_norm(a)
  37. {
  38.     return a.s^2 + dp(a.v, a.v);
  39. }
  40.  
  41.  
  42. define quat_abs(a, e)
  43. {
  44.     return sqrt(a.s^2 + dp(a.v, a.v), e);
  45. }
  46.  
  47.  
  48. define quat_conj(a)
  49. {
  50.     local obj quat    x;
  51.  
  52.     x.s = a.s;
  53.     x.v = -a.v;
  54.     return x;
  55. }
  56.  
  57.  
  58. define quat_add(a, b)
  59. {
  60.     local obj quat    x;
  61.  
  62.     if (!istype(b, x)) {
  63.         x.s = a.s + b;
  64.         x.v = a.v;
  65.         return x;
  66.     }
  67.     if (!istype(a, x)) {
  68.         x.s = a + b.s;
  69.         x.v = b.v;
  70.         return x;
  71.     }
  72.     x.s = a.s + b.s;
  73.     x.v = a.v + b.v;
  74.     if (x.v)
  75.         return x;
  76.     return x.s;
  77. }
  78.  
  79.  
  80. define quat_sub(a, b)
  81. {
  82.     local obj quat    x;
  83.  
  84.     if (!istype(b, x)) {
  85.         x.s = a.s - b;
  86.         x.v = a.v;
  87.         return x;
  88.     }
  89.     if (!istype(a, x)) {
  90.         x.s = a - b.s;
  91.         x.v = -b.v;
  92.         return x;
  93.     }
  94.     x.s = a.s - b.s;
  95.     x.v = a.v - b.v;
  96.     if (x.v)
  97.         return x;
  98.     return x.s;
  99. }
  100.  
  101.  
  102. define quat_inc(a)
  103. {
  104.     local    x;
  105.  
  106.     x = a;
  107.     x.s++;
  108.     return x;
  109. }
  110.  
  111.  
  112. define quat_dec(a)
  113. {
  114.     local    x;
  115.  
  116.     x = a;
  117.     x.s--;
  118.     return x;
  119. }
  120.  
  121.  
  122. define quat_neg(a)
  123. {
  124.     local obj quat    x;
  125.  
  126.     x.s = -a.s;
  127.     x.v = -a.v;
  128.     return x;
  129. }
  130.  
  131.  
  132. define quat_mul(a, b)
  133. {
  134.     local obj quat    x;
  135.  
  136.     if (!istype(b, x)) {
  137.         x.s = a.s * b;
  138.         x.v = a.v * b;
  139.     } else if (!istype(a, x)) {
  140.         x.s = b.s * a;
  141.         x.v = b.v * a;
  142.     } else {
  143.         x.s = a.s * b.s - dp(a.v, b.v);
  144.         x.v = a.s * b.v + b.s * a.v + cp(a.v, b.v);
  145.     }
  146.     if (x.v)
  147.         return x;
  148.     return x.s;
  149. }
  150.  
  151.  
  152. define quat_div(a, b)
  153. {
  154.     local obj quat    x;
  155.  
  156.     if (!istype(b, x)) {
  157.         x.s = a.s / b;
  158.         x.v = a.v / b;
  159.         return x;
  160.     }
  161.     return a * quat_inv(b);
  162. }
  163.  
  164.  
  165. define quat_inv(a)
  166. {
  167.     local    x, q2;
  168.  
  169.     obj quat x;
  170.     q2 = a.s^2 + dp(a.v, a.v);
  171.     x.s = a.s / q2;
  172.     x.v = a.v / (-q2);
  173.     return x;
  174. }
  175.  
  176.  
  177. define quat_scale(a, b)
  178. {
  179.     local obj quat    x;
  180.  
  181.     x.s = scale(a.s, b);
  182.     x.v = scale(a.v, b);
  183.     return x;
  184. }
  185.  
  186.  
  187. define quat_shift(a, b)
  188. {
  189.     local obj quat    x;
  190.  
  191.     x.s = a.s << b;
  192.     x.v = a.v << b;
  193.     if (x.v)
  194.         return x;
  195.     return x.s;
  196. }
  197.  
  198. global lib_debug;
  199. if (lib_debug >= 0) {
  200.     print "obj quat {s, v} defined";
  201.     print "quat(a, b, c, d) defined";
  202.     print "quat_print(a) defined";
  203.     print "quat_norm(a) defined";
  204.     print "quat_abs(a, e) defined";
  205.     print "quat_conj(a) defined";
  206.     print "quat_add(a, e) defined";
  207.     print "quat_sub(a, e) defined";
  208.     print "quat_inc(a) defined";
  209.     print "quat_dec(a) defined";
  210.     print "quat_neg(a) defined";
  211.     print "quat_mul(a, b) defined";
  212.     print "quat_div(a, b) defined";
  213.     print "quat_inv(a) defined";
  214.     print "quat_scale(a, b) defined";
  215.     print "quat_shift(a, b) defined";
  216. }
  217.